/* Includes ----------------------------------------------------------------- */
#include "../bsp/bsp_tim.h"
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */

typedef struct {
  float pwm_l;
  float pwm_r;
  float pwm_server;
  uint32_t last_waketime;

} PidPwm_t;

typedef struct {
  float out;
  float integral;
  float error_last;
} PidOut_t;

typedef struct {
  float k;
  float kp;
  float ki;
  float kd;
} Pid_t;

typedef struct {
  Pid_t pid;
  PidOut_t out;

} Pid_Param_t;

/* Exported functions prototypes -------------------------------------------- */

float PID_AngleSpeedLoop(float anglespeed_want, float angle_speed,
                         Pid_Param_t *pid_p);
float PID_AngleLoop(float angle_want, float angle, Pid_Param_t *pid_p);
float PID_SpeedLoop(float speed_want, float speed_now, Pid_Param_t *pid_p);
float PID_TurnLoop(float turn_angel_want, float turn_angel_now,
                   Pid_Param_t *pid_p);
#ifdef __cplusplus
}
#endif
